
#include <stdio.h>
#include <math.h>
#include "e53_me_Fir.h"
#include "lz_hardware.h"
#include "string.h"
#define FIR_FILTER_LENGTH 129 //滤波的阶数+1 （128阶）
// 带通滤波器系数：采样频率为250Hz，截止频率为3Hz~30Hz 通过filterDesigner获取
const float LPF_50Hz[FIR_FILTER_LENGTH]  =
{
    3.523997657e-05, 0.0002562592272, 0.0005757701583, 0.0008397826459, 0.000908970891,
    0.0007304374012, 0.0003793779761, 4.222582356e-05, -6.521392788e-05, 0.0001839015895,
    0.0007320778677, 0.001328663086, 0.001635892317, 0.001413777587, 0.0006883906899,
    -0.0002056905651, -0.0007648666506, -0.0005919140531, 0.0003351111372, 0.001569915912,
    0.002375603188, 0.002117323689, 0.0006689901347, -0.001414557919, -0.003109993879,
    -0.003462586319, -0.00217742566, 8.629632794e-05, 0.001947802957, 0.002011778764,
    -0.0002987752669, -0.004264956806, -0.00809297245, -0.009811084718, -0.008411717601,
    -0.004596390296, -0.0006214127061, 0.0007985962438, -0.001978532877, -0.008395017125,
    -0.01568987407, -0.02018531598, -0.01929843985, -0.01321159769, -0.005181713495,
    -0.0001112028476, -0.001950757345, -0.01125541423,  -0.0243169684, -0.03460548073,
    -0.03605531529, -0.02662901953, -0.01020727865, 0.004513713531, 0.008002913557,
    -0.004921500571, -0.03125274926, -0.05950148031, -0.07363011688, -0.05986980721,
    -0.01351031102,  0.05752891302,   0.1343045086,   0.1933406889,   0.2154731899,
    0.1933406889,   0.1343045086,  0.05752891302, -0.01351031102, -0.05986980721,
    -0.07363011688, -0.05950148031, -0.03125274926, -0.004921500571, 0.008002913557,
    0.004513713531, -0.01020727865, -0.02662901953, -0.03605531529, -0.03460548073,
    -0.0243169684, -0.01125541423, -0.001950757345, -0.0001112028476, -0.005181713495,
    -0.01321159769, -0.01929843985, -0.02018531598, -0.01568987407, -0.008395017125,
    -0.001978532877, 0.0007985962438, -0.0006214127061, -0.004596390296, -0.008411717601,
    -0.009811084718, -0.00809297245, -0.004264956806, -0.0002987752669, 0.002011778764,
    0.001947802957, 8.629632794e-05, -0.00217742566, -0.003462586319, -0.003109993879,
    -0.001414557919, 0.0006689901347, 0.002117323689, 0.002375603188, 0.001569915912,
    0.0003351111372, -0.0005919140531, -0.0007648666506, -0.0002056905651, 0.0006883906899,
    0.001413777587, 0.001635892317, 0.001328663086, 0.0007320778677, 0.0001839015895,
    -6.521392788e-05, 4.222582356e-05, 0.0003793779761, 0.0007304374012, 0.000908970891,
    0.0008397826459, 0.0005757701583, 0.0002562592272, 3.523997657e-05
};
float state[FIR_FILTER_LENGTH + 1] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
// //-----------------------------------------------------------------
// // void process(float *x,float *y,int N)
// //-----------------------------------------------------------------
// //
// // 函数功能: FIR滤波
// // 入口参数: x:输入的数据 y：输出的数据 N:待处理的数据长度
// // 返 回 值: 无
// // 注意事项: 无
// //
// //-----------------------------------------------------------------
void e53_me_process(float *x, float *y, int N)
{
    int i = 0, j = 0, k = 0;
    double temp;
    for (k = 0; k < N; k++)
    {
        state[0] = x[k];
        for (i = 0, temp = 0; i < FIR_FILTER_LENGTH; i++)
        {
            temp += LPF_50Hz[i] * state[i];
        }
        y[k] = temp;
        for (j = FIR_FILTER_LENGTH - 1; j > -1 ; j--)
        {
            state[j + 1] = state[j];
        }
    }
}

/*
p_input : 待滤波数据
p_output ： 滤波输出数据
data_len ： 数据长度
p_fir_para ： h(t)参数
fir_para_len ： h(t)长度
*/
// void c_fir(float* p_input, float* p_output, uint16_t data_len, float* p_fir_para,uint16_t fir_para_len)
// {
//     memset(p_output, 0, sizeof(float) * data_len);  //输出清零

//     for (uint16_t i = 0; i < data_len; i++)
//     {
//         if (i < fir_para_len)
//         {
//             for (uint16_t j = 0; j <= i; j++)
//             {
//                 p_output[i] += p_fir_para[j] * p_input[i - j];
//             }
//         }
//         else if(i >= fir_para_len)
//         {
//             for (uint16_t j = 0; j < fir_para_len; j++)
//             {
//                 p_output[i] += p_fir_para[j] * p_input[i - j];  //卷积和
//             }

//         }
//     }
// }
